/* ===========头文件==============================================*/
#include "UpctrlTask.h"
#include "rcs.h"

#include "RCS_dsp.h"
#include "RCS_Scara.h"

#include "Ch_Ctrl.h"
#include "Up_Ctrl.h"
#include "RCS_MOTOR.h"

#include "Valve_2006.h"
/* ============私有全局变量========================================*/
extern Motor_Ctrl_Node RM3508_Test;//引用其他文件的结点需要，声明测试结点
extern Motor_Ctrl_Node RM2006_Test;
extern Motor_Ctrl_Node Shoot1_Test;
extern Motor_Ctrl_Node Shoot2_Test;//3508

extern Motor_Ctrl_Node Launch_Shoot_Mirror_1;//3508
extern Motor_Ctrl_Node Launch_Shoot_Mirror_2;
extern Motor_Ctrl_Node Launch_Feed_Sync_1;//2006
extern Motor_Ctrl_Node Launch_Feed_Sync_2;//2006

RCS_PIN_USART USART_WIFI;

OS_STK   upctrl_task_stk[NORMAL_TASK_STK_SIZE];
volatile uint16_t ctrl_time_us;
uint8_t status = 0;
/* ============用户全局变量========================================*/
volatile uint8_t blink_var;

/* ============机器人主任务========================================*/
void upctrl_task(void *p_arg)
{
	p_arg = p_arg;
	while(1)
	{
		/*----------更新调试变量----------------------------------*/

		/*----------更新机器人控制指令----------------------------*/
		//AGV_Re_Center_Move_v3_3(0.0f, 20.0f, 0.0f);	//舵轮底盘直接控制
		//BaseMove_Joystick_Test();						//舵轮底盘手柄控制
		MotorNode_Update_Spd(-6000, &Shoot1_Test);
		MotorNode_Update_Spd(6000, &Shoot2_Test);		//摩擦轮发射3508控制

		MotorNode_Update_Spd(-800, &Launch_Feed_Sync_1);
		MotorNode_Update_Spd(-800, &Launch_Feed_Sync_2);//播弹盘2006控制
        //Demo_RMESC_Test_Old();						//老代码电机测试
		
//		switch(status)									//发射机构往复播弹逻辑
//		{
//			case 0:
//			{
//				MotorNode_Update_Spd(800, &Launch_Feed_Sync_1);
//				MotorNode_Update_Spd(800, &Launch_Feed_Sync_2);
//				if(Count_Delay(1000, 1))
//				{
//					status = 1;
//				}
//			}
//			case 1:
//			{
//				MotorNode_Update_Spd(-800, &Launch_Feed_Sync_1);
//				MotorNode_Update_Spd(-800, &Launch_Feed_Sync_2);
//				if(Count_Delay(1200, 2))
//				{
//					status = 0;
//				}
//			}
//		}
        /*----------下发机器人控制指令----------------------------*/
        MotorList_Excute(CAN_GROUP_1); // 将电机控制指令下发
		MotorList_Excute(CAN_GROUP_2); // 将电机控制指令下发
        /*-----------200Hz控制频率-------------------------------*/
        RCS_Hard_Stamper_Disable();
        if (blink_var==0){GPIO_SetBits(GPIOD,GPIO_Pin_3);}
		if (blink_var==1){GPIO_ResetBits(GPIOD,GPIO_Pin_3);}
		if (Count_Delay(200,1)) 
		{
			if (blink_var==0){blink_var=1;}
			else if (blink_var==1){blink_var=0;}
		}
		ctrl_time_us=Hard_Get_RealTime_Stamp();
		delay_ms(UPCTRL_TASK_CYCLE_MS);//如无必要不应改动
	}
}

/* =============主任务初始化======================================*/
void UpCtrlTask_Init()
{
	/*-----自定义初始化--------------------------------------*/
    //RM2006_Test_Init();					//2006电机测试初始化
    //RM3508_Test_Init();					//3508电机测试初始化
    //MotorNode_Shoot_Init();				
    //Demo_MotorTest_Init_Old();			//老代码电机测试初始化
    //AGV_Init();							//舵轮底盘电机初始化
	
//	USART_WIFI.GPIOx = GPIOA;
//	USART_WIFI.GPIO_Pin_Rx = GPIO_Pin_9;
//	USART_WIFI.GPIO_Pin_Tx = GPIO_Pin_10;
//	USART_WIFI.USARTx = USART1;
//	BlueTooth_Init(USART_WIFI);				//USART初始化
	
//	RM3508_Shoot_Test_Init();				//3508摩擦轮发射测试初始化
//	Feed_Sync_Test_Init();					//2006播弹盘电机测试初始化
	Launch_Shoot_Init();					//发射机构电机初始化
    /*-----RTOS任务初始化-------------------------------------*/
    OSTaskCreate(upctrl_task,
                 (void *)0,
                 &upctrl_task_stk[NORMAL_TASK_STK_SIZE - 1],
                 upctrl_task_PRIO);
}



